#include "DEM.h"

DEM::DEM(/* args */)
{
}

DEM::~DEM()
{
}


PointCloud DEM::Getwarppc()
{
    return Pc_warp_;
}

Eigen::Vector2d DEM::GetOc()
{
    return Origin_coor_;
}

std::map<std::pair<int,int>,Scalar> DEM::GetMap_e()
{
    return Map_elvation;
}

std::map<std::pair<int,int>,Eigen::Vector3d> DEM::GetMap_c()
{
    return Map_color;
}

/*TODO更新函数*/
void DEM::Merge(MatrixXX Smat_X,DEM* dem)
{
    //1.更新标定坐标
    Eigen::Vector2d old_O = this->GetOc();
    this->Origin_coor_ = dem->Origin_coor_;
    //2.根据形变信息更新Map
    auto mape = dem->GetMap_e();
    auto mapc = dem->GetMap_c();
    std::map<std::pair<int,int>,Scalar>            Map_elvation_new;
    std::map<std::pair<int,int>,Eigen::Vector3d>   Map_color_new;
    size_t rowid = 0;
 
    for(std::map<std::pair<int,int>,Scalar>::iterator it = Map_elvation.begin();it!=Map_elvation.end();it++)
    {
        auto id = it->first;
        Scalar z = it->second;
        Scalar z_chage = z+Smat_X.row(rowid)(2);
        Eigen::Vector3d color = Map_color.find(id)->second;
        //更新id
        int vx = id.first+floor((Smat_X.row(rowid)(0)+old_O(0)-Origin_coor_(0))/this->resolution_);
        int vy = id.second+floor((Smat_X.row(rowid)(1)+old_O(1)-Origin_coor_(1))/this->resolution_);
        std::pair<int,int> id_new(vx,vy);
        //融合高程值
        if(mape.find(id_new)==mape.end())//如果没找到，则直接添加到新的map中
        {
            Map_elvation_new.emplace(std::pair<std::pair<int,int>,Scalar>(id_new,z_chage));
            Map_color_new.emplace(std::pair<std::pair<int,int>,Eigen::Vector3d>(id_new,color));
        }
        // else
        // {
        //     Scalar znew = mape.find(id_new)->second;
        //     Eigen::Vector3d cnew = mapc.find(id_new)->second;
        //     //融合，融合策略后续需要修改
        //     Map_elvation_new.emplace(std::pair<std::pair<int,int>,Scalar>(id_new,znew));
        //     Map_color_new.emplace(std::pair<std::pair<int,int>,Eigen::Vector3d>(id_new,cnew));
        // }
        rowid++;
    }
    Map_color_new.insert(mapc.begin(),mapc.end());
    Map_elvation_new.insert(mape.begin(),mape.end());
    Map_color = Map_color_new;
    Map_elvation = Map_elvation_new;


}

void DEM::Merge_GPU(Scalar *deformpositon,DEM* dem, int n_allp)//这里输入的是所有点形变后的位置以及点的数量
{

    this->Origin_coor_ = dem->Origin_coor_;

    /*更新map*/
    auto mape = dem->GetMap_e();
    auto mapc = dem->GetMap_c(); 
    std::map<std::pair<int,int>,Scalar>            Map_elvation_new;
    std::map<std::pair<int,int>,Eigen::Vector3d>   Map_color_new;

    int i = 0;
    for(std::map<std::pair<int,int>,Eigen::Vector3d>::iterator it = Map_color.begin();it!=Map_color.end();it++)
    {   

        //计算点的新id
        int x_new = std::floor((deformpositon[i]-Origin_coor_(0))/resolution_);
        int y_new = std::floor((deformpositon[i+n_allp]-Origin_coor_(1))/resolution_);
        std::pair<int,int> id_new(x_new,y_new);
        
        if(mape.find(id_new)==mape.end())//如果没找到，则直接添加到新的map中
        {
            Map_elvation_new.emplace(std::pair<std::pair<int,int>,Scalar>(id_new,deformpositon[i+2*n_allp]));
            Map_color_new.emplace(std::pair<std::pair<int,int>,Eigen::Vector3d>(id_new,it->second));
        }
        i++;
    }

    Map_color.clear();
    Map_elvation.clear();
    Map_color_new.insert(mapc.begin(),mapc.end());
    Map_elvation_new.insert(mape.begin(),mape.end());
    Map_color = Map_color_new;
    Map_elvation = Map_elvation_new;
    


}


void DEM::ExtractWarppc(Scalar radius)
{
    radius_ = radius;
    PointCloud pc;//
    PointCloud pc_all = this->Generate_pc();
    //梯度选点
  
    pc.points_.push_back(pc_all.points_[0]);    

    //距离选点
    for(size_t i=0;i<pc_all.points_.size();i++)
    {
        open3d::geometry::KDTreeFlann* tree = new open3d::geometry::KDTreeFlann(pc);
        std::vector<int> index;
        std::vector<Scalar> dis2;
        tree->SearchKNN(pc_all.points_[i],1,index,dis2);
        if(dis2[0]>std::pow(radius,2))
        {
            pc.points_.push_back(pc_all.points_[i]);
        }
    }
    Pc_warp_ = pc;
    

}


void DEM::Grad_cal(std::map<std::pair<int,int>,Scalar> map)
{
    Scalar d_a = 0;
    Scalar d_b = 0;
    Scalar d_c = 0;
    Scalar d_d = 0;
    Scalar d_e = 0;
    Scalar d_f = 0;
    Scalar d_g = 0;
    Scalar d_h = 0;
    for(std::map<std::pair<int,int>,Scalar>::iterator it=Map_elvation.begin();it!=Map_elvation.end();it++)
    {
        auto hash = it->first;
        Scalar d_p = it->second;
        int row = hash.first; int col=hash.second;
        std::pair<int,int> neighbor_hash1(row+1,col);
        std::pair<int,int> neighbor_hash2(row+1,col+1);
        std::pair<int,int> neighbor_hash3(row+1,col-1);
        std::pair<int,int> neighbor_hash4(row-1,col);
        std::pair<int,int> neighbor_hash5(row-1,col+1);
        std::pair<int,int> neighbor_hash6(row-1,col-1);
        std::pair<int,int> neighbor_hash7(row,col+1);
        std::pair<int,int> neighbor_hash8(row,col-1);

        if(Map_elvation.find(neighbor_hash1)!= Map_elvation.end())
        {
            d_f = Map_elvation.find(neighbor_hash1)->second;
        }
        if(Map_elvation.find(neighbor_hash2)!= Map_elvation.end())
        {
            d_e = Map_elvation.find(neighbor_hash2)->second;
        }
        if(Map_elvation.find(neighbor_hash3)!= Map_elvation.end())
        {
            d_g = Map_elvation.find(neighbor_hash3)->second;
        }
        if(Map_elvation.find(neighbor_hash4)!= Map_elvation.end())
        {
            d_b = Map_elvation.find(neighbor_hash4)->second;
        }
        if(Map_elvation.find(neighbor_hash5)!= Map_elvation.end())
        {
            d_c = Map_elvation.find(neighbor_hash5)->second;
        }
        if(Map_elvation.find(neighbor_hash6)!= Map_elvation.end())
        {
            d_a = Map_elvation.find(neighbor_hash6)->second;
        }
        if(Map_elvation.find(neighbor_hash7)!= Map_elvation.end())
        {
            d_d = Map_elvation.find(neighbor_hash7)->second;
        }
        if(Map_elvation.find(neighbor_hash8)!= Map_elvation.end())
        {
            d_h = Map_elvation.find(neighbor_hash8)->second;
        }

        Scalar dae = abs(d_p-(d_a+d_e)*0.5);
        Scalar dbf = abs(d_p-(d_b+d_f)*0.5);
        Scalar dcg = abs(d_p-(d_c+d_g)*0.5);
        Scalar ddh = abs(d_p-(d_d+d_h)*0.5);
        Scalar grad_eva = (dae+dbf+dcg+ddh)*0.25;//高程梯度值

        Map_grad.emplace(std::pair<std::pair<int,int>,Scalar>(hash,grad_eva));

    }



}

void DEM::DEM_generate(std::shared_ptr<open3d::geometry::PointCloud> pc,Scalar gridsize)
{
    std::multimap<std::pair<int,int>,int> pointsinGrid_id;//网格id->点id

    //计算点云包围盒并分割盒子
    Scalar xMax = pc->GetMaxBound()[0];
    Scalar xMin = pc->GetMinBound()[0];
    Scalar yMax = pc->GetMaxBound()[1];
    Scalar yMin = pc->GetMinBound()[1];
    resolution_ = gridsize;
    Origin_coor_<<xMin,yMin;

    int nrow = ceil((xMax-xMin)/gridsize);
    int ncol = ceil((yMax-yMin)/gridsize);
    height_ = nrow*resolution_;
    wight_ = ncol*resolution_;

    //统计点云
    // #pragma omp parallel for
    for(size_t index =0; index< pc->points_.size(); index++)
    {
        int rowID = std::floor(pc->points_[index](0)-xMin)/gridsize;
        int colID = std::floor(pc->points_[index](1)-yMin)/gridsize;
        std::pair<int,int> hash = std::make_pair(rowID,colID);
        pointsinGrid_id.insert({hash,index});
    }

    for(size_t i = 0;i<nrow; i++)
    {
        for(size_t j =0;j<ncol; j++)
        {   
            //平均点和平均颜色
            Eigen::Vector3d average_point,average_color;
            average_point.setZero();
            average_color.setZero();

            auto hash = std::make_pair(i,j);

            int npoints = 0;//该方格中点的数量
            for(auto pos = pointsinGrid_id.equal_range(hash);pos.first!=pos.second;pos.first++)
            {
                average_color += pc->colors_[pos.first->second];
                average_point(2) += pc->points_[pos.first->second](2);
                npoints ++;
            }
            if(npoints!=0)
            {
                average_color /= npoints;
                average_point(2) /= npoints;
                Map_elvation.emplace(std::pair<std::pair<int,int>,Scalar>(hash,average_point(2)));
                Map_color.emplace(std::pair<std::pair<int,int>,Eigen::Vector3d>(hash,average_color));
            }
        }
    }

    Grad_cal(Map_elvation); 

}

void DEM::Coffset_GPU(double* &f,int f_ld,double* &ew, int ew_ld, double* &m, int m_ld,VPairs corrs,Scalar searchratio,double* &x_)
{
    open3d::geometry::KDTreeFlann* tree = new open3d::geometry::KDTreeFlann(Pc_warp_);
    std::vector<Scalar> Vweight;
    for(int i=0;i<corrs.size();i++)
    {
        Vweight.clear();
        Scalar w_sum = 0;
        auto p = corrs[i].position_src;
        std::vector<int> index;
        std::vector<Scalar> dis2;
        tree->SearchRadius(p,searchratio*radius_,index,dis2);//对于每一个匹配点搜索邻域网格点,搜索距离1.1*radius**********非常重要，需要调整***********
        for(auto& distence:dis2)
        {
            Scalar weight = std::pow(1 - distence*std::pow(1.0/(2*radius_), 2), 3);
            w_sum += weight;
            Vweight.push_back(weight);
        }
        for(int j =0;j<Vweight.size();j++)
        {
            Scalar rightw = Vweight[j]/w_sum;
            f[i+index[j]*f_ld] =rightw; 
        }
    }

    for(int k =0;k<Pc_warp_.points_.size();k++)
    {
        std::vector<int> index;
        std::vector<Scalar> dis2;
        Vweight.clear();
        Scalar w_sum = 0;
        tree->SearchRadius(Pc_warp_.points_[k],searchratio*radius_,index,dis2);
        for(auto& distence:dis2)
        {
            if(distence!=0)
            {
                Scalar weight = std::pow(1 - distence*std::pow(1.0/(2*radius_), 2), 3);
                w_sum += weight;
                Vweight.push_back(weight);
            }
            else
            {
                Vweight.push_back(1.0);
            }
  
        }

        for(int h =0;h<Vweight.size();h++)
        {
            
            if(k == index[h])
            {
                ew[k+index[h]*ew_ld]=1;
            }
            else
            {
                Scalar rightw = Vweight[h]/w_sum;
                ew[k+index[h]*ew_ld]=-rightw;
            }
        }
    }

    auto pc_all = this->Generate_pc();
    for(int i =0;i<pc_all.points_.size();i++)
    {
        Vweight.clear();
        Scalar w_sum = 0;
        auto p = pc_all.points_[i];
        std::vector<int> index;
        std::vector<Scalar> dis2;
        tree->SearchRadius(p,1.1*radius_,index,dis2);//搜索距离1.1*radius**********非常重要，需要调整***********
        
        for(auto& distence:dis2)
        {
            Scalar weight = std::pow(1 - distence*std::pow(1.0/(2*radius_), 2), 3);
            w_sum += weight;
            Vweight.push_back(weight);
        }
        for(int j =0;j<Vweight.size();j++)
        {
            Scalar rightw = Vweight[j]/w_sum;
            m[i+index[j]*m_ld] = rightw;
        }        

    }

    //初始化x
    PointCloud pc_corres;
    for(auto& c:corrs)
    {
        pc_corres.points_.push_back(c.position_src); 
    }
    open3d::geometry::KDTreeFlann* tree2 = new open3d::geometry::KDTreeFlann(pc_corres);
    int i = 0;
    for(auto& p:Pc_warp_.points_)
    {
        std::vector<int> index;
        std::vector<Scalar> dis2;
        tree2->SearchKNN(p,1,index,dis2);
        x_[i] = corrs[index[0]].position_tar(0) - corrs[index[0]].position_src(0);
        x_[i+ew_ld] = corrs[index[0]].position_tar(1) - corrs[index[0]].position_src(1);
        x_[i+ew_ld*2] = corrs[index[0]].position_tar(2) - corrs[index[0]].position_src(2);
        i++;
    }
}


/*输出三维模型*/
PointCloud DEM::Generate_pc()
{
    PointCloud Pc_DEM;
    Eigen::Vector3d average_point,average_color;
    for(std::map<std::pair<int,int>,Scalar>::iterator it=Map_elvation.begin();it!=Map_elvation.end();it++)
    {
        auto hash = it->first;
        int row = hash.first;
        int col = hash.second;
        auto z = it->second;
        average_point(0) = (row+0.5)*resolution_ + Origin_coor_(0);
        average_point(1) = (col+0.5)*resolution_ + Origin_coor_(1);
        average_point(2) = z;
        average_color = Map_color.find(hash)->second;
        Pc_DEM.points_.push_back(average_point);
        Pc_DEM.colors_.push_back(average_color);
    }

    return Pc_DEM;
}

open3d::geometry::TriangleMesh DEM::Generate_mesh()
{
    PointCloud pc_dem = this->Generate_pc();
    Scalar xMax = pc_dem.GetMaxBound()[0];
    Scalar xMin = pc_dem.GetMinBound()[0];
    Scalar yMax = pc_dem.GetMaxBound()[1];
    Scalar yMin = pc_dem.GetMinBound()[1];
    int nrow = ceil((xMax-xMin)/resolution_);
    int ncol = ceil((yMax-yMin)/resolution_);
    height_ = nrow*resolution_;
    wight_ = ncol*resolution_;
    


    std::map<std::pair<float,float>,int> coor2index;//存放二维点到index的映射
    open3d::geometry::TriangleMesh mesh;
    mesh.vertex_colors_ = pc_dem.colors_;
    mesh.vertices_ = pc_dem.points_;
    //创建二维分割对象，放入分割点云
    cv::Rect rect(xMin-1,yMin-1,height_+1,wight_+1);
    cv::Subdiv2D sub_div(rect);

    int index = 0;
    for(auto& p:pc_dem.points_)
    {
        cv::Point2f p_2d={float(p(0)),float(p(1))};
        std::pair<float,float> pairfloat = std::make_pair(float(p(0)),float(p(1)));
        sub_div.insert(p_2d);
        coor2index.insert({pairfloat,index});
        index++;
    }
    std::vector<cv::Vec6f> trilist;
    sub_div.getTriangleList(trilist);
    for(auto& tri:trilist)
    {   
        
        std::pair<float,float> pairfloat1 = std::make_pair(tri(0),tri(1));
        std::pair<float,float> pairfloat2 = std::make_pair(tri(2),tri(3));
        std::pair<float,float> pairfloat3 = std::make_pair(tri(4),tri(5));

        auto it1 = coor2index.find(pairfloat1);
        auto it2 = coor2index.find(pairfloat2);
        auto it3 = coor2index.find(pairfloat3);
        int index1 = it1->second;
        int index2 = it2->second;
        int index3 = it3->second;
        Eigen::Vector3i triangle = {index1,index2,index3};
        mesh.triangles_.push_back(triangle);

    }
    return mesh;
}